#include "Components/ATK_MS901M/atk_ms901m.h"
#include "Components/algorithm.h"
#include "Components/basic.h"
#include "Components/line_patrol.h"
#include "Components/motion.h"
#include "Components/motor.h"
#include "Components/oled_hardware_i2c.h"
#include "Components/servo.h"
#include "Components/simple_json.h"
#include <ti/driverlib/m0p/dl_interrupt.h>
#include "Components/clock.h"
#include <math.h>

#define TAG "Main AT "

int main(void)
{
  SYSCFG_DL_init();
  SysTick_Init();

  uart_init();
  OLED_Init();
  motion_init();
  OLED_ShowString(0, 0, "Bus Servo Test", 16);

  log(TAG, "Bus servo Start");

#define ID 45

  bus_servo_set_id(ID);
  mspm0_delay_ms(500);

  bus_servo_set_mode(ID, BUS_SERVO_CLOCKWISE_180_MODE);
  mspm0_delay_ms(500);

  log(TAG, "INIT DONE");

  while (1)
  {
    bus_servo_set_pwm(ID, 0, 1000);
    mspm0_delay_ms(2000);
    bus_servo_set_pwm(ID, 180, 1000);
    mspm0_delay_ms(2000);
  }
}